#include "maths/shape.h"

#include "maths/ray.h"
#include "maths/sphere.h"
#include "maths/aabbox.h"

#include "maths/utils.h"

namespace dw {
	namespace maths {

// http://www.geometrictools.com/LibMathematics/Intersection/Intersection.html

/*static*/ bool Shape::intersects(const Shape& shapeA, const Shape& shapeB)
{
	switch (shapeA.type)
	{
	case Type_Ray :
		{
			switch (shapeB.type)
			{
			case Type_Ray :
				return Shape::intersects(static_cast< const Ray& >(shapeA), static_cast< const Ray& >(shapeB));
			case Type_Sphere :
				return Shape::intersects(static_cast< const Ray& >(shapeA), static_cast< const Sphere& >(shapeB));
			case Type_AABBox :
				return Shape::intersects(static_cast< const Ray& >(shapeA), static_cast< const AABBox& >(shapeB));
			default :
				break ;
			}
		}
		break ;
	case Type_Sphere :
		{
			switch (shapeB.type)
			{
			case Type_Ray :
				return Shape::intersects(static_cast< const Sphere& >(shapeA), static_cast< const Ray& >(shapeB));
			case Type_Sphere :
				return Shape::intersects(static_cast< const Sphere& >(shapeA), static_cast< const Sphere& >(shapeB));
			case Type_AABBox :
				return Shape::intersects(static_cast< const Sphere& >(shapeA), static_cast< const AABBox& >(shapeB));
			default :
				break ;
			}
		}
		break ;
	case Type_AABBox :
		{
			switch (shapeB.type)
			{
			case Type_Ray :
				return Shape::intersects(static_cast< const AABBox& >(shapeA), static_cast< const Ray& >(shapeB));
			case Type_Sphere :
				return Shape::intersects(static_cast< const AABBox& >(shapeA), static_cast< const Sphere& >(shapeB));
			case Type_AABBox :
				return Shape::intersects(static_cast< const AABBox& >(shapeA), static_cast< const AABBox& >(shapeB));
			default :
				break ;
			}
		}
		break ;
	default :
		break ;
	}

	dwFail("Invalid shapes pair");

	return false;
}

/*static*/ bool Shape::intersects(const Shape& shapeA, const Shape& shapeB, Intersection& intersection)
{
	switch (shapeA.type)
	{
	case Type_Ray :
		{
			switch (shapeB.type)
			{
			case Type_Ray :
				return Shape::intersects(static_cast< const Ray& >(shapeA), static_cast< const Ray& >(shapeB), intersection);
			case Type_Sphere :
				return Shape::intersects(static_cast< const Ray& >(shapeA), static_cast< const Sphere& >(shapeB), intersection);
			case Type_AABBox :
				return Shape::intersects(static_cast< const Ray& >(shapeA), static_cast< const AABBox& >(shapeB), intersection);
			default :
				break ;
			}
		}
		break ;
	case Type_Sphere :
		{
			switch (shapeB.type)
			{
			case Type_Ray :
				return Shape::intersects(static_cast< const Sphere& >(shapeA), static_cast< const Ray& >(shapeB), intersection);
			case Type_Sphere :
				return Shape::intersects(static_cast< const Sphere& >(shapeA), static_cast< const Sphere& >(shapeB), intersection);
			case Type_AABBox :
				return Shape::intersects(static_cast< const Sphere& >(shapeA), static_cast< const AABBox& >(shapeB), intersection);
			default :
				break ;
			}
		}
		break ;
	case Type_AABBox :
		{
			switch (shapeB.type)
			{
			case Type_Ray :
				return Shape::intersects(static_cast< const AABBox& >(shapeA), static_cast< const Ray& >(shapeB), intersection);
			case Type_Sphere :
				return Shape::intersects(static_cast< const AABBox& >(shapeA), static_cast< const Sphere& >(shapeB), intersection);
			case Type_AABBox :
				return Shape::intersects(static_cast< const AABBox& >(shapeA), static_cast< const AABBox& >(shapeB), intersection);
			default :
				break ;
			}
		}
		break ;
	default :
		break ;
	}

	dwFail("Invalid shapes pair");

	return false;
}

// Ray / Ray

/*static*/ bool Shape::intersects(const Ray& shapeA, const Ray& shapeB)
{
	// dwFail("Not implemented");

	return false;
}

/*static*/ bool Shape::intersects(const Ray& shapeA, const Ray& shapeB, Intersection& intersection)
{
	// dwFail("Not implemented");

	return false;
}

// Ray / Sphere

/*static*/ bool Shape::intersects(const Ray& shapeA, const Sphere& shapeB)
{
	dwAssert(shapeB.radius > 0, "Invalid parameter: shapeB's radius is null");
	
	// A*t^2 + B*t + C = 0
	// with:
	// A = Xd^2 + Yd^2 + Zd^2
	// B = 2 * (Xd * (X0 - Xc) + Yd * (Y0 - Yc) + Zd * (Z0 - Zc))
	// C = (X0 - Xc)^2 + (Y0 - Yc)^2 + (Z0 - Zc)^2 - Sr^2

	Vector3f rayFromSphere = shapeA.position - shapeB.position;

	const float32 a = dw::maths::Vector3f::dot(shapeA.direction, shapeA.direction);
	const float32 b = 2.0f * Vector3f::dot(shapeA.direction, rayFromSphere);
	const float32 c = Vector3f::dot(rayFromSphere, rayFromSphere) - shapeB.radius * shapeB.radius;

	// Discriminant of quadratic = b^2 - 4 * a * c
	const float32 delta =  b * b - 4.0f * a * c;
	
	// Check for intersection with Sphere
	if (delta < 0.0f)
	{
		return false;
	}

	float32 sqrtDiscriminant = std::sqrt(delta);

	float32 depth;

	if ((depth = ((-b - sqrtDiscriminant) / (2.0f * a))) >= 0.0f ||
		(depth = ((-b + sqrtDiscriminant) / (2.0f * a))) >= 0.0f)
	{
		if (depth <= 1.0f)
		{
			return true;
		}
	}

	return false;
}

/*static*/ bool Shape::intersects(const Ray& shapeA, const Sphere& shapeB, Intersection& intersection)
{
	dwAssert(shapeB.radius > 0, "Invalid parameter: shapeB's radius is null");
	
	Vector3f rayDir = shapeA.direction;

	rayDir.normalize();

	// A*t^2 + B*t + C = 0
	// with:
	// A = Xd^2 + Yd^2 + Zd^2
	// B = 2 * (Xd * (X0 - Xc) + Yd * (Y0 - Yc) + Zd * (Z0 - Zc))
	// C = (X0 - Xc)^2 + (Y0 - Yc)^2 + (Z0 - Zc)^2 - Sr^2

	Vector3f rayFromSphere = shapeA.position - shapeB.position;

	const float32 a = dw::maths::Vector3f::dot(shapeA.direction, shapeA.direction);
	const float32 b = 2.0f * Vector3f::dot(shapeA.direction, rayFromSphere);
	const float32 c = Vector3f::dot(rayFromSphere, rayFromSphere) - shapeB.radius * shapeB.radius;

	// Discriminant of quadratic = b^2 - 4 * a * c
	const float32 delta =  b * b - 4.0f * a * c;
	
	// Check for intersection with Sphere
	if (delta < 0.0f)
	{
		return false;
	}

	float32 sqrtDiscriminant = std::sqrt(delta);

	float32 depth;

	if ((depth = ((-b - sqrtDiscriminant) / (2.0f * a))) >= 0.0f ||
		(depth = ((-b + sqrtDiscriminant) / (2.0f * a))) >= 0.0f)
	{
		if (depth <= 1.0f)
		{
			intersection.depth = depth * shapeA.direction.length();
			intersection.position = shapeA.position + depth * shapeA.direction;
			intersection.normalA = rayDir;
			intersection.normalB = (intersection.position - shapeB.position).normalize();
			intersection.type = Intersection::Type_Intersects;

			return true;
		}
	}

	intersection.type = Intersection::Type_None;

	return false;
}

// Ray / AABBox

/*static*/ bool Shape::intersects(const Ray& shapeA, const AABBox& shapeB)
{
	// dwFail("Not implemented");

	return false;
}

/*static*/ bool Shape::intersects(const Ray& shapeA, const AABBox& shapeB, Intersection& intersection)
{
	// dwFail("Not implemented");

	return false;
}

// Sphere / Ray

/*static*/ bool Shape::intersects(const Sphere& shapeA, const Ray& shapeB)
{
	return Shape::intersects(shapeB, shapeA);
}

/*static*/ bool Shape::intersects(const Sphere& shapeA, const Ray& shapeB, Intersection& intersection)
{
	if (Shape::intersects(shapeB, shapeA, intersection))
	{
		Shape::swapIntersectionResult(intersection);

		return true;
	}

	return false;
}

// Sphere / Sphere

/*static*/ bool Shape::intersects(const Sphere& shapeA, const Sphere& shapeB)
{
	dwAssert(shapeA.radius > 0, "Invalid parameter: shapeA's radius is null");
	dwAssert(shapeB.radius > 0, "Invalid parameter: shapeB's radius is null");
	
	Vector3f aTob = shapeB.position - shapeA.position;
	float32 radi = shapeA.radius + shapeB.radius;

	return aTob.sqrLength() < (radi * radi);
}

/*static*/ bool Shape::intersects(const Sphere& shapeA, const Sphere& shapeB, Intersection& intersection)
{
	dwAssert(shapeA.radius > 0, "Invalid parameter: shapeA's radius is null");
	dwAssert(shapeB.radius > 0, "Invalid parameter: shapeB's radius is null");
	
	Vector3f aTob = shapeB.position - shapeA.position;
	float32 totalRadius = shapeA.radius + shapeB.radius;
	float32 sqrLength = aTob.sqrLength();

	float32 sqrDepth = (totalRadius * totalRadius) - sqrLength;

	if (sqrDepth > 0.0f)
	{
		float32 length = std::sqrt( sqrLength );

		if (length == 0.0f) // Shapes are at the same exact position
		{
			intersection.normalA = Vector3f(0.0f,  1.0f, 0.0f);
			intersection.normalB = Vector3f(0.0f, -1.0f, 0.0f);

			intersection.depth = totalRadius;
		}
		else
		{
			intersection.normalA = aTob;
			intersection.normalA.normalize();
			intersection.normalB = -intersection.normalA;

			intersection.depth = totalRadius - length;
		}

		if (length + shapeB.radius < shapeA.radius)
		{
			intersection.type = Intersection::Type_IncludesB;
		}
		else if (length + shapeA.radius < shapeB.radius)
		{
			intersection.type = Intersection::Type_IncludesA;
		}
		else
		{
			intersection.type = Intersection::Type_Intersects;
		}

		intersection.position = shapeA.position + shapeA.radius * intersection.normalA;

		return true;
	}
	
	intersection.type = Intersection::Type_None;

	return false;
}

// Sphere / AABBox

/*static*/ bool Shape::intersects(const Sphere& shapeA, const AABBox& shapeB)
{
	dwAssert(shapeA.radius > 0, "Invalid parameter: shapeA's radius is null");
	dwAssert(shapeB.halfExtend.x > 0, "Invalid parameter: shapeB's width is null");
	dwAssert(shapeB.halfExtend.y > 0, "Invalid parameter: shapeB's height is null");
	dwAssert(shapeB.halfExtend.z > 0, "Invalid parameter: shapeB's length is null");
	
	Vector3f boxMin = shapeB.position - shapeB.halfExtend;
	Vector3f boxMax = shapeB.position + shapeB.halfExtend;
	Vector3f closestPoint = Vector3f::clamp(shapeA.position, boxMin, boxMax);
	Vector3f closestPointFromSphere = closestPoint - shapeA.position;

	return (closestPointFromSphere.sqrLength() < dw::maths::square(shapeA.radius));
}

/*static*/ bool Shape::intersects(const Sphere& shapeA, const AABBox& shapeB, Intersection& intersection)
{
	dwAssert(shapeA.radius > 0, "Invalid parameter: shapeA's radius is null");
	dwAssert(shapeB.halfExtend.x > 0, "Invalid parameter: shapeB's width is null");
	dwAssert(shapeB.halfExtend.y > 0, "Invalid parameter: shapeB's height is null");
	dwAssert(shapeB.halfExtend.z > 0, "Invalid parameter: shapeB's length is null");

	Vector3f boxMin = shapeB.position - shapeB.halfExtend;
	Vector3f boxMax = shapeB.position + shapeB.halfExtend;
	Vector3f closestPoint = Vector3f::clamp(shapeA.position, boxMin, boxMax);
	Vector3f closestPointFromSphere = closestPoint - shapeA.position;

	float32 sqrDepth = closestPointFromSphere.sqrLength();

	if (sqrDepth < dw::maths::square(shapeA.radius))
	{
		if (sqrDepth == 0.0f) // sphere's center is inside box
		{
			Vector3f centerFromBox = shapeA.position - shapeB.position;
			float32 sqrLength = centerFromBox.sqrLength();

			if (sqrLength == 0.0f) // Shapes are at the same exact position
			{
				intersection.normalA = Vector3f(0.0f,  1.0f, 0.0f);

				intersection.depth = shapeA.radius + shapeB.halfExtend.y;
			}
			else
			{
				int32 mainAxis = 0;
				float32 bestFit = 0.0f;
				float32 currentFit = 0.0f;
				float32 value;

				Vector3f normalizedAxis;

				normalizedAxis.x = centerFromBox.x / shapeB.halfExtend.x;
				normalizedAxis.y = centerFromBox.y / shapeB.halfExtend.y;
				normalizedAxis.z = centerFromBox.z / shapeB.halfExtend.z;

				for (int32 axis = 0; axis < 3; ++axis)
				{
					currentFit = std::abs(normalizedAxis[axis]);
					if (currentFit > bestFit)
					{
						bestFit = currentFit;
						mainAxis = axis;
						value = (normalizedAxis[axis] < 0.0f) ? -1.0f : 1.0f;
					}
				}

				intersection.normalA.reset();
				intersection.normalA[mainAxis] = -value;

				intersection.depth = (1.0f - bestFit) * shapeB.halfExtend[mainAxis] + shapeA.radius;
			}
		}
		else
		{
			intersection.normalA = closestPointFromSphere;
			intersection.normalA.normalize();
			
			intersection.depth = shapeA.radius - closestPointFromSphere.length();
		}

		intersection.normalB = -intersection.normalA;

		intersection.type = Intersection::Type_Intersects;

		intersection.position = shapeA.position + shapeA.radius * intersection.normalA;

		return true;
	}

	intersection.type = Intersection::Type_None;

	return false;
}

// AABBox / Ray

/*static*/ bool Shape::intersects(const AABBox& shapeA, const Ray& shapeB)
{
	return Shape::intersects(shapeB, shapeA);
}

/*static*/ bool Shape::intersects(const AABBox& shapeA, const Ray& shapeB, Intersection& intersection)
{
	if (Shape::intersects(shapeB, shapeA, intersection))
	{
		Shape::swapIntersectionResult(intersection);

		return true;
	}

	return false;
}

// AABBox / Sphere

/*static*/ bool Shape::intersects(const AABBox& shapeA, const Sphere& shapeB)
{
	return Shape::intersects(shapeB, shapeA);
}

/*static*/ bool Shape::intersects(const AABBox& shapeA, const Sphere& shapeB, Intersection& intersection)
{
	if (Shape::intersects(shapeB, shapeA, intersection))
	{
		Shape::swapIntersectionResult(intersection);

		return true;
	}

	return false;
}

// AABBox / AABBox

/*static*/ bool Shape::intersects(const AABBox& shapeA, const AABBox& shapeB)
{
	// dwFail("Not implemented");

	return false;
}

/*static*/ bool Shape::intersects(const AABBox& shapeA, const AABBox& shapeB, Intersection& intersection)
{
	// dwFail("Not implemented");

	return false;
}

/*static*/ void Shape::swapIntersectionResult(Intersection& intersection)
{
	dwAssert(intersection.type != Intersection::Type_None, "Invalid intersection type");

	std::swap(intersection.normalA, intersection.normalB);

	if (intersection.type != Intersection::Type_Intersects)
	{
		intersection.type = ( intersection.type == Intersection::Type_IncludesA )
			? Intersection::Type_IncludesB
			: Intersection::Type_IncludesA;
	}
}

Vector3f Shape::getClosestPoint(const dw::maths::Vector3f &p, const dw::maths::Ray& shape)
{
	dwFail("Not implemented");

	return Vector3f::zero;
}

Vector3f Shape::getClosestPoint(const dw::maths::Vector3f &p, const dw::maths::Sphere& shape)
{
	dwFail("Not implemented");

	return Vector3f::zero;
}

Vector3f Shape::getClosestPoint(const dw::maths::Vector3f &p, const dw::maths::AABBox& shape)
{
	dwFail("Not implemented");

	return Vector3f::zero;
}

	} // namespace maths
} // namespace dw
